
#define BOOST_TEST_MODULE MMM_KINEMATIC_SENSOR_TEST

#include "MMM/RapidXML/RapidXMLReader.h"
#include "MMM/Motion/Plugin/KinematicPlugin/KinematicSensor.h"
#include "MMM/Model/Model.h"
#include "MMM/Exceptions.h"
#include "TestHelper.h"

#include <boost/test/unit_test.hpp>

#include <iostream>

BOOST_AUTO_TEST_CASE( LoadSensor )
{
    MMM::KinematicSensorPtr sensor = MMM::KinematicSensor::loadSensorXML(MMM::RapidXMLReader::FromXmlString(
                "<?xml version='1.0' encoding='utf-8'?>"
                "<Sensor type='Kinematic'>"
                "   <Configuration>"
                "       <Joint name='BLNx_joint'/>"
                "       <Joint name='BLNy_joint'/>"
                "       <Joint name='BLNz_joint'/>"
                "   </Configuration>"
                "   <Data>"
                "       <Measurement timestep='0.0'>"
                "           <JointPosition>1.0 1.0 -1.0</JointPosition>"
                "       </Measurement>"
                "       <Measurement timestep='1.0'>"
                "           <JointPosition>2.0 0.0 1.0</JointPosition>"
                "       </Measurement>"
                "   </Data>"
                "</Sensor>")->getRoot("Sensor"));

    std::vector<std::string> jointNames{"BLNx_joint", "BLNy_joint", "BLNz_joint"};
    BOOST_REQUIRE(sensor->getJointNames() == jointNames);

    MMM::KinematicSensorMeasurementPtr sensorMeasurement = sensor->getDerivedMeasurement(0.0);
    BOOST_REQUIRE(sensorMeasurement);
    BOOST_REQUIRE(!sensorMeasurement->isInterpolated());
    BOOST_REQUIRE(sensorMeasurement->getJointAngles().isApprox(Eigen::Vector3f(1.0f, 1.0f, -1.0f)));

    sensorMeasurement = sensor->getDerivedMeasurement(1.0);
    BOOST_REQUIRE(sensorMeasurement);
    BOOST_REQUIRE(!sensorMeasurement->isInterpolated());
    BOOST_REQUIRE(sensorMeasurement->getJointAngles().isApprox(Eigen::Vector3f(2.0f, 0.0f, 1.0f)));
}

BOOST_AUTO_TEST_CASE( LoadSensor_XMLException)
{
    // Duplicate Joint
    BOOST_CHECK_THROW(MMM::KinematicSensor::loadSensorXML(MMM::RapidXMLReader::FromXmlString(
                "<?xml version='1.0' encoding='utf-8'?>"
                "<Sensor type='Kinematic'>"
                "   <Configuration>"
                "       <Joint name='BLNx_joint'/>"
                "       <Joint name='BLNx_joint'/>"
                "   </Configuration>"
                "   <Data>"
                "       <Measurement timestep='0.0'>"
                "           <JointPosition>1.0 1.0</JointPosition>"
                "       </Measurement>"
                "       <Measurement timestep='1.0'>"
                "           <JointPosition>2.0 0.0</JointPosition>"
                "       </Measurement>"
                "   </Data>"
                "</Sensor>")->getRoot("Sensor")), MMM::Exception::XMLFormatException);

    // Empty configuration
    BOOST_CHECK_THROW(MMM::KinematicSensor::loadSensorXML(MMM::RapidXMLReader::FromXmlString(
                "<?xml version='1.0' encoding='utf-8'?>"
                "<Sensor type='Kinematic'>"
                "   <Configuration/>"
                "   <Data>"
                "       <Measurement timestep='0.0'>"
                "           <JointPosition></JointPosition>"
                "       </Measurement>"
                "   </Data>"
                "</Sensor>")->getRoot("Sensor")), MMM::Exception::XMLFormatException);

    // Too few joint angles
    BOOST_CHECK_THROW(MMM::KinematicSensor::loadSensorXML(MMM::RapidXMLReader::FromXmlString(
                "<?xml version='1.0' encoding='utf-8'?>"
                "<Sensor type='Kinematic'>"
                "   <Configuration>"
                "       <Joint name='BLNx_joint'/>"
                "       <Joint name='BLNy_joint'/>"
                "   </Configuration>"
                "   <Data>"
                "       <Measurement timestep='0.0'>"
                "           <JointPosition>1.0</JointPosition>"
                "       </Measurement>"
                "   </Data>"
                "</Sensor>")->getRoot("Sensor")), MMM::Exception::XMLFormatException);

    // Too much joint angles
    BOOST_CHECK_THROW(MMM::KinematicSensor::loadSensorXML(MMM::RapidXMLReader::FromXmlString(
                "<?xml version='1.0' encoding='utf-8'?>"
                "<Sensor type='Kinematic'>"
                "   <Configuration>"
                "       <Joint name='BLNx_joint'/>"
                "       <Joint name='BLNy_joint'/>"
                "   </Configuration>"
                "   <Data>"
                "       <Measurement timestep='0.0'>"
                "           <JointPosition>1.0 1.0 1.0</JointPosition>"
                "       </Measurement>"
                "   </Data>"
                "</Sensor>")->getRoot("Sensor")), MMM::Exception::XMLFormatException);

    // No Measurements
    BOOST_CHECK_THROW(MMM::KinematicSensor::loadSensorXML(MMM::RapidXMLReader::FromXmlString(
                "<?xml version='1.0' encoding='utf-8'?>"
                "<Sensor type='Kinematic'>"
                "   <Configuration>"
                "       <Joint name='BLNx_joint'/>"
                "       <Joint name='BLNy_joint'/>"
                "   </Configuration>"
                "   <Data>"
                "       <Measurement timestep='0.0'>"
                "           <JointPosition>1.0 1.0 1.0</JointPosition>"
                "       </Measurement>"
                "   </Data>"
                "</Sensor>")->getRoot("Sensor")), MMM::Exception::XMLFormatException);
}

BOOST_AUTO_TEST_CASE( WriteSensor )
{
    std::vector<std::string> jointNames{"BLNx_joint", "BLNy_joint", "BLNz_joint"};
    MMM::KinematicSensorPtr sensor(new MMM::KinematicSensor(jointNames));

    MMM::KinematicSensorMeasurementPtr sensorMeasurement1(new MMM::KinematicSensorMeasurement(0.0, Eigen::Vector3f(1.0f, 1.0f, -1.0f)));
    sensor->addSensorMeasurement(sensorMeasurement1);

    MMM::KinematicSensorMeasurementPtr sensorMeasurement2(new MMM::KinematicSensorMeasurement(1.0, Eigen::Vector3f(2.0f, 0.0f, 1.0f)));
    sensor->addSensorMeasurement(sensorMeasurement2);

    MMM::RapidXMLWriterPtr writer(new MMM::RapidXMLWriter());
    MMM::RapidXMLWriterNodePtr rootNode = writer->createRootNode("XMLString");
    sensor->appendSensorXML(rootNode);

    MMM::KinematicSensorPtr writtenSensor = MMM::KinematicSensor::loadSensorXML(MMM::RapidXMLReader::FromXmlString(writer->print(false))->getRoot()->first_node("Sensor"));

    BOOST_REQUIRE(sensor->equalsConfiguration(writtenSensor));
    BOOST_REQUIRE(sensor->getMeasurement(0.0)->equals(sensorMeasurement1));
    BOOST_REQUIRE(sensor->getMeasurement(1.0)->equals(sensorMeasurement2));
}

BOOST_AUTO_TEST_CASE( EqualsSensorConfiguration )
{
    MMM::KinematicSensorPtr sensor1(new MMM::KinematicSensor(std::vector<std::string> {"BLNx_joint", "BLNy_joint"}));
    MMM::KinematicSensorPtr sensor2(new MMM::KinematicSensor(std::vector<std::string> {"BLNx_joint", "BLNy_joint"}));
    MMM::KinematicSensorPtr sensor3(new MMM::KinematicSensor(std::vector<std::string> {"BLNx_joint", "BLNy_joint", "BLNz_joint"}));

    BOOST_REQUIRE(sensor1->equalsConfiguration(sensor1));
    BOOST_REQUIRE(sensor1->equalsConfiguration(sensor2));
    BOOST_REQUIRE(!sensor1->equalsConfiguration(sensor3));
}

BOOST_AUTO_TEST_CASE( EqualsSensorMeasurement )
{
    MMM::KinematicSensorMeasurementPtr sensorMeasurement1(new MMM::KinematicSensorMeasurement(0.0, Eigen::Vector2f(1.0f, 1.0f)));
    MMM::KinematicSensorMeasurementPtr sensorMeasurement2(new MMM::KinematicSensorMeasurement(0.5, Eigen::Vector2f(1.0f, 1.0f), MMM::SensorMeasurementType::INTERPOLATED));
    MMM::KinematicSensorMeasurementPtr sensorMeasurement3(new MMM::KinematicSensorMeasurement(1.0, Eigen::Vector2f(-1.0f, 3.0f)));

    BOOST_REQUIRE(sensorMeasurement1->equals(sensorMeasurement1));
    BOOST_REQUIRE(sensorMeasurement1->equals(sensorMeasurement2));
    BOOST_REQUIRE(!sensorMeasurement1->equals(sensorMeasurement3));
}

BOOST_AUTO_TEST_CASE( CloneSensor )
{
    MMM::KinematicSensorPtr sensor(new MMM::KinematicSensor(std::vector<std::string> {"BLNx_joint", "BLNy_joint"}));

    MMM::KinematicSensorMeasurementPtr sensorMeasurement1(new MMM::KinematicSensorMeasurement(0.0, Eigen::Vector2f(1.0f, 1.0f)));
    sensor->addSensorMeasurement(sensorMeasurement1);

    MMM::KinematicSensorMeasurementPtr sensorMeasurement2(new MMM::KinematicSensorMeasurement(1.0, Eigen::Vector2f(-1.0f, 3.0f)));
    sensor->addSensorMeasurement(sensorMeasurement2);

    boost::shared_ptr<MMM::BasicSensor<MMM::KinematicSensorMeasurement> > clonedSensor = sensor->cloneDerived();

    BOOST_REQUIRE(clonedSensor->equalsConfiguration(sensor));
    BOOST_REQUIRE(sensor->getDerivedMeasurement(0.0)->equals(sensorMeasurement1));
    BOOST_REQUIRE(sensor->getDerivedMeasurement(1.0)->equals(sensorMeasurement2));
}

BOOST_AUTO_TEST_CASE( CheckModel )
{
    MMM::KinematicSensorPtr sensor1(new MMM::KinematicSensor(std::vector<std::string> {"BLNx_joint", "BLNy_joint"}));
    MMM::KinematicSensorPtr sensor2(new MMM::KinematicSensor(std::vector<std::string> {"BLNx_joint", "BLNy_joint", "BLNz_joint"}));

    MMM::ModelPtr model = TestHelper::createMockupModel(std::set<std::string> {"BLNx_joint", "BLNy_joint"});

    BOOST_REQUIRE(sensor1->checkModel(model));
    BOOST_REQUIRE(!sensor2->checkModel(model));
}

BOOST_AUTO_TEST_CASE( LinearInterpolation )
{
    MMM::KinematicSensorPtr sensor(new MMM::KinematicSensor(std::vector<std::string> {"BLNx_joint", "BLNy_joint"}));

    MMM::KinematicSensorMeasurementPtr sensorMeasurement1(new MMM::KinematicSensorMeasurement(0.0, Eigen::Vector2f(1.0f, 1.0f)));
    sensor->addSensorMeasurement(sensorMeasurement1);

    MMM::KinematicSensorMeasurementPtr sensorMeasurement2(new MMM::KinematicSensorMeasurement(1.0, Eigen::Vector2f(-1.0f, 3.0f)));
    sensor->addSensorMeasurement(sensorMeasurement2);

    MMM::KinematicSensorMeasurementPtr sensorMeasurement = sensor->getDerivedMeasurement(0.5);
    BOOST_REQUIRE(sensorMeasurement);
    BOOST_REQUIRE(sensorMeasurement->isInterpolated());
    BOOST_REQUIRE(sensorMeasurement->getJointAngles().isApprox(Eigen::Vector2f(0.0f, 2.0f)));
}


// BOOST_AUTO_TEST_SUITE_END()
